/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of the University of California, Berkeley nor the
 *    names of its contributors may be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * @file forklift_controller.cpp
 * @author gc
 * @date June 26, 2023
 * @brief forklift operation
 * Contact: congcong.guan@lgmgim.cn
 */

#include <agv_peripheral_controller/fork/fork_controller.h>

using namespace std;

namespace agv_peripheral
{

	double getVariance(double var[], int n)
	{
		int i = 0;
		double sum = 0.0;
		double avg = 0.0;
		for (i=0; i<n; i++) {
			sum += var[i];
		}

		avg = sum/(double)n;

		sum = 0.0;
		for (i=0; i<n; i++) {
			sum += (var[i] - avg) * (var[i] - avg);
		}

		return sum/(double)(n-1);
	}

	ForkController::ForkController()
	{
		init();
	}

	void ForkController::init()
	{
		state = IDLE;
		find_joint_id = false;
		target_vel = 0.0;
		target_pos = 0.0;
    	forklift_pub = nh_.advertise<std_msgs::msg::Float64>("/base/controllers/forkLift_controller/command", 1);
		jointstate_sub = nh_.subscribe("/base/joint_states", 10, &ForkController::jointStateCallback, this);
	    trigger_server = nh_.advertiseService("/follow_trigger", &ForkController::followTriggercallback, this);

	    int type;
		nh_.param("/agv_peripheral_controller/PeripheralType", type, 4);
		peripheral_type = (PeriphType)type;


		nh_.param<double>("/agv_peripheral_controller/ForkLiftMaxPos", max_pos, 2.0);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftMinPos", min_pos, 0.1);

		nh_.param<int>("/agv_peripheral_controller/ForkLiftUPComp", up_comp, 30);
		nh_.param<int>("/agv_peripheral_controller/ForkLiftDOWNComp", down_comp, 30);

		/*
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlVKP", v_kp, 20.0);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlVKI", v_ki, 10.0);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlVKD", v_kd, 10.0);
		*/

		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlPKP", p_kp, 100.0);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlPKI", p_ki, 5.0);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlPKD", p_kd, 5.0);

		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlAcc", forklift_control_acc, 30);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlVel", forklift_control_vel, 80);

		nh_.param<double>("/agv_peripheral_controller/KP", kp, 100.0);
		nh_.param<double>("/agv_peripheral_controller/KI", ki, 1.0);
		nh_.param<double>("/agv_peripheral_controller/KD", kd, 0.0);
		nh_.param<double>("/agv_peripheral_controller/Upper_limit", upper_limit, 100.0);
		nh_.param<double>("/agv_peripheral_controller/Lower_limit", lower_limit, -100.0);
		nh_.param<double>("/agv_peripheral_controller/Integral_limit", integral_limit, 30.0);		
		nh_.param<double>("/agv_peripheral_controller/Integral_threshold", integral_threshold, 0.3);

		pid_controller.pid_param_.Kp = kp;
		pid_controller.pid_param_.Ki = ki;
		pid_controller.pid_param_.Kd = kd;
		pid_controller.pid_param_.period = 0.05;
		pid_controller.pid_param_.upper_limit = upper_limit;
		pid_controller.pid_param_.lower_limit = lower_limit;
		pid_controller.pid_param_.integral_limit = integral_limit;
		pid_controller.pid_param_.integral_threshold = integral_threshold;
		pid_controller.pid_param_.inverted = false;		
	}



	bool ForkController::followTriggercallback(agv_srvs::srv::FollowTrigger::Request& req, agv_srvs::srv::FollowTrigger::Response& resp)
	{
	    resp.status = true;
	    set_target_position(req.target);
	    // set_target_velocity(req.target);
	    RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORK CONTROLLER] test triggered,  p[%.4f] i[%.4f] d[%.4f]", p_kp, p_ki, p_kd);
	    return true;
	}


	void ForkController::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg)
	{
		joint_state = *msg;
		if (!find_joint_id) {
			for (int i = 0; i < joint_state.name.size(); i++) {
				if (joint_state.name[i].find(FORKLIFT_JOINT) != string::npos) {
					forkjoint_id = i;
					find_joint_id = true;
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORK CONTROLLER] FORKLIFT JOINT ID:%d", forkjoint_id);
				}
			}

			if (!find_joint_id) {
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[FORK CONTROLLER] Could not find FORKLIFT JOINT number");
			}
		}

		if (find_joint_id) {
			jointState_update_time = rclcpp::Time::now();
			current_pos = joint_state.position[forkjoint_id];
			// current_vel = joint_state.velocity[forkjoint_id];		


			for (int i=4; i>0; i--) {
				pos_table[i] = pos_table[i-1];
			}

			pos_table[0] = current_pos;
			current_vel = (pos_table[0] - pos_table[4]) / (0.05 * 4.0);
		}
	}

	void ForkController::set_target_position(double pos, double acc)
	{
		nh_.param<int>("/agv_peripheral_controller/ForkLiftUPComp", up_comp, 30);
		nh_.param<int>("/agv_peripheral_controller/ForkLiftDOWNComp", down_comp, 30);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlAcc", forklift_control_acc, 30);
		nh_.param<double>("/agv_peripheral_controller/ForkLiftControlVel", forklift_control_vel, 80);

		nh_.param<double>("/agv_peripheral_controller/KP", kp, 100.0);
		nh_.param<double>("/agv_peripheral_controller/KI", ki, 1.0);
		nh_.param<double>("/agv_peripheral_controller/KD", kd, 0.0);
		nh_.param<double>("/agv_peripheral_controller/Upper_limit", upper_limit, 100.0);
		nh_.param<double>("/agv_peripheral_controller/Lower_limit", lower_limit, -100.0);
		nh_.param<double>("/agv_peripheral_controller/Integral_limit", integral_limit, 30.0);		
		nh_.param<double>("/agv_peripheral_controller/Integral_threshold", integral_threshold, 0.3);
		pid_controller.pid_param_.Kp = kp;
		pid_controller.pid_param_.Ki = ki;
		pid_controller.pid_param_.Kd = kd;
		pid_controller.pid_param_.period = 0.05;
		pid_controller.pid_param_.upper_limit = upper_limit;
		pid_controller.pid_param_.lower_limit = lower_limit;
		pid_controller.pid_param_.integral_limit = integral_limit;
		pid_controller.pid_param_.integral_threshold = integral_threshold;				
		if (state == IDLE) {
			target_set_time = rclcpp::Time::now();
			target_pos = pos;			
			state = TASK;
			for(int i=0; i<20; i++)
				pos_err[i] = 0.0;

			/*
			deadz_detected = false;
			if (current_pos < target_pos && deadz_up > DEADZ_DECLINE)
				deadz_up -= DEADZ_DECLINE;
			if (current_pos > target_pos && deadz_down < (-1 * DEADZ_DECLINE))
				deadz_down += DEADZ_DECLINE;
			*/
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORKLIFT CONTROLLER] set target position: %.4f  start time: %.4f", pos, target_set_time.toSec());
		} else {
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORKLIFT CONTROLLER] controller busy. state[%d]", state);
		}
	}

	void ForkController::set_target_velocity(double vel)
	{
		if (state == IDLE || state == MANUAL) {
			state = MANUAL;
			target_vel = vel;
			// output = 0;
			target_vel = target_vel > LIFT_MAX_SPEED_UP ? LIFT_MAX_SPEED_UP : target_vel;
			target_vel = target_vel < LIFT_MAX_SPEED_DOWN ? LIFT_MAX_SPEED_DOWN : target_vel;

			if (abs(target_vel) < 0.0001)
				stop();
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORKLIFT CONTROLLER] set target velocity: %.4f", target_vel);
		} else {
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORKLIFT CONTROLLER] controller busy. state[%d]", state);
		}
	}

	/*
	void ForkController::forkLiftVelocityControl()
	{
		rclcpp::Time now = rclcpp::Time::now();
		for (int i=9; i>0; i--) {
			vel_err[i] = vel_err[i-1]; 
		}

		vel_err[0] = target_vel - current_vel;

		vel_err_integral = 0.0;
		for (int j=0; j<6; j++) 
			vel_err_integral += vel_err[j];

		double inc_d = vel_err[0] * v_kp + vel_err_integral * v_ki + (vel_err[0] - vel_err[1]) * v_kd;
		int inc_i = 0;
		if (abs(inc_d) < 0.1)
			inc_i = 0;
		else
			inc_i = inc_d > 0 ? ceil(inc_d) : floor(inc_d);

		output = output < 0 ? output + 2 * inc_i : output + inc_i;
		output = output > LIFT_MAX_SPEED_UP ? LIFT_MAX_SPEED_UP : output;
		output = output < LIFT_MAX_SPEED_DOWN ? LIFT_MAX_SPEED_DOWN : output;

		forklift_data.data = output;
		forklift_pub.publish(forklift_data);

		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "pos: %.4f  t_vel: %.4f  c_vel: %.4f  err: %.4f  inta: %.4f  dif: %.4f  output: %d", current_pos, target_vel, current_vel, vel_err[0] * v_kp, 
			vel_err_integral * v_ki, (vel_err[0] - vel_err[1]) * v_kd, output);
	}
	*/

	void ForkController::forkLiftPositionControl()
	{
		rclcpp::Time now = rclcpp::Time::now();

		// for(int i=19; i>0; i--) {
		// 	pos_err[i] = pos_err[i-1];
		// }
 
		// pos_err[0] = target_pos - current_pos;


		// pos_err_integral = 0.0;
		// for (int j=0; j<20; j++)
		// 	pos_err_integral += pos_err[j];

		double vel_limit = (now - target_set_time).toSec() * forklift_control_acc;

		//double vel = pos_err[0] * p_kp + pos_err_integral * p_ki + (pos_err[0] - pos_err[1]) * p_kd;
		double vel = pid_controller.positionPID(current_pos, target_pos, false, false);
		/* dynamic dead zone compensate

		double variance = 10000 * getVariance(pos_err, 20);

       	RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[FORKLIFT CONTROLLER] t: %.4f c: %.4f  t_speed: %d  va: %.6f  v_limit: %.4f  d_up: %d  d_down: %d c_vel: %.4f", 
       		target_pos, current_pos, vel, variance, vel_limit, deadz_up, deadz_down, current_vel);

       	if (!deadz_detected) {
			if (vel > 0 && variance < 0.0001 && abs(vel) < vel_limit) {
				deadz_up = deadz_up > vel ? deadz_up : vel;
				deadz_detected = true;
			}

			if (vel < 0 && variance < 0.0050 && abs(vel) < vel_limit) {
				deadz_up = deadz_up > vel ? deadz_up : vel;
				deadz_detected = true;
			}
		}
	
       	if (vel > 0)
       		vel += deadz_up;
       	if (vel < 0)
       		vel += deadz_down;
		*/
		// RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "vel: %.4f  vel_limit: %.4f", vel, vel_limit);

		ROS_INFO_STREAM_THROTTLE(0.2, "pid output: " <<vel);
       	vel = vel > vel_limit ? vel_limit : vel;
       	vel = vel < (-1 * vel_limit) ? (-1 * vel_limit) : vel;

		if (vel > 0)
			vel += up_comp;
		if (vel < 0) {
			vel -= down_comp;
		}
       	vel = vel > forklift_control_vel ? forklift_control_vel : vel;
       	vel = vel < (-1 * forklift_control_vel) ? (-1 * forklift_control_vel) : vel;
     	
		ROS_INFO_THROTTLE(0.2, "pos: %.4f  target: %.4f   output: %d", current_pos, target_pos,  (int)vel);
     	// target_vel = vel;
       	// forkLiftVelocityControl();

		forklift_data.data = (int)vel;
		forklift_pub.publish(forklift_data);
	}

	void ForkController::stop()
	{
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "stop");
		state = IDLE;
		// output = 0;
		pid_controller.positionPID(0.0, 0.0, false, true);
		forklift_data.data = 0;
		forklift_pub.publish(forklift_data);
	}

	void ForkController::update()
	{
		switch (state) {
			case IDLE:
				break;
			case TASK:
				forkLiftPositionControl();
				if (abs(target_pos - current_pos) < 0.005) {
					stop();
				}
				break;
			case MANUAL:
				// forkLiftVelocityControl();
				if (peripheral_type == FORKLIFT && ((current_pos >= max_pos && target_vel > 0) || (current_pos <= min_pos && target_vel < 0)))
				 	stop();
				else {
					forklift_data.data = (int)target_vel;
					forklift_pub.publish(forklift_data);
				}
				break;
			default:
				break;
		}

	}

}
